/// 投影图像，按照角度保存到 test 目录中
/// ./Vtk_ProjectionImage /works/src_works/z_make/testdata/head_s5 1 1000
/// 1 为读取实际dicom文件夹
/// 1000 为threshold
///
#include <vector>

#include <vtkSmartPointer.h>
#include <vtkTransform.h>
#include <vtkMatrix4x4.h>
#include <vtkMetaImageReader.h>
#include <vtkImageData.h>
#include <vtkDICOMImageReader.h>
#include <itkRescaleIntensityImageFilter.h>

#include "itkImage.h"
#include "itkImageFileWriter.h"

#include <Eigen/Core>


using namespace std;

Eigen::Vector3d multiplyPoint(vtkMatrix4x4 *m, Eigen::Vector3d &p)
{
    Eigen::Vector3d outP;
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    outP[0] = v1 * elem[0] + v2 * elem[1] + v3 * elem[2];
    outP[1] = v1 * elem[4] + v2 * elem[5] + v3 * elem[6] ;
    outP[2] = v1 * elem[8] + v2 * elem[9] + v3 * elem[10];

    return outP;
}

Eigen::Vector3d multiplyPoint(vtkMatrix4x4 *m, double p[3])
{
    Eigen::Vector3d outP;
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    outP[0] = v1 * elem[0] + v2 * elem[1] + v3 * elem[2] + elem[3];
    outP[1] = v1 * elem[4] + v2 * elem[5] + v3 * elem[6] + elem[7];
    outP[2] = v1 * elem[8] + v2 * elem[9] + v3 * elem[10]+ elem[11];

    return outP;
}

// 计算位置，并偏移到合适的位置
Eigen::Vector2d projectionPoint(vtkMatrix4x4 *m, double p[3], double u0, double v0)
{
    Eigen::Vector2d outP;
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    double Zc = v1 * elem[8] + v2 * elem[9] + v3 * elem[10]+ elem[11];

    outP[0] = (v1 * elem[0] + v2 * elem[1] + v3 * elem[2] + elem[3])/Zc + u0;
    outP[1] = (v1 * elem[4] + v2 * elem[5] + v3 * elem[6] + elem[7])/Zc + v0;


    return outP;
}

// 计算位置，并偏移到合适的位置
void projectionPoint(vtkMatrix4x4 *m, double p[3], double u0, double v0, double outP[2])
{
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    double Zc = v1 * elem[8] + v2 * elem[9] + v3 * elem[10]+ elem[11];

    outP[0] = (v1 * elem[0] + v2 * elem[1] + v3 * elem[2] + elem[3])/Zc + u0;
    outP[1] = (v1 * elem[4] + v2 * elem[5] + v3 * elem[6] + elem[7])/Zc + v0;
}

/// x,y,z 传入的是弧度脚, 偏移量
vtkSmartPointer<vtkMatrix4x4> createMatrixEulerZYX(Eigen::Vector3d &anglesInRadian, Eigen::Vector3d &center, Eigen::Vector3d &translation)
{
    vtkSmartPointer<vtkMatrix4x4> m = vtkMatrix4x4::New();

    double u = anglesInRadian[2];
    double v = anglesInRadian[1];
    double w = anglesInRadian[0];
    /**
     u, v, w 绕 z, y, x
    [
        cv * cu     sw*sv*cu - cw*su   cw*sv*cu + sw*su
        cv * su     sw*sv*su + cw*cu   cw*sv*su - sw*cu
        -sv         sw * cv            cw * cv
    ]
     */

    double cu = cos(u);
    double su = sin(u);
    double cv = cos(v);
    double sv = sin(v);
    double cw = cos(w);
    double sw = sin(w);

    m->Element[0][0] = cv * cu;
    m->Element[1][0] = cv * su;
    m->Element[2][0] = -sv;
    m->Element[3][0] = 0;

    m->Element[0][1] = sw*sv*cu - cw*su;
    m->Element[1][1] = sw*sv*su + cw*cu;
    m->Element[2][1] = sw * cv;
    m->Element[3][1] = 0;

    m->Element[0][2] = cw*sv*cu + sw*su;
    m->Element[1][2] = cw*sv*su - sw*cu;
    m->Element[2][2] = cw * cv;
    m->Element[3][2] = 0;

    m->Element[0][3] = 0;
    m->Element[1][3] = 0;
    m->Element[2][3] = 0;
    m->Element[3][3] = 1;

    Eigen::Vector3d offset = multiplyPoint(m, center);

    m->Element[0][3] = translation[0]-offset[0];
    m->Element[1][3] = translation[1]-offset[1];
    m->Element[2][3] = translation[2]-offset[2];
    m->Modified();

    return m;
}

vtkSmartPointer<vtkMatrix4x4> createHMatrix(Eigen::Vector3d &center, Eigen::Vector3d &translation,
                                            double f, double dx, double dy,
                                            Eigen::Vector3d &anglesInDegree)
{

    Eigen::Vector3d anglesInRadian = anglesInDegree * M_PI/ 180.0;
    vtkSmartPointer<vtkMatrix4x4> m = createMatrixEulerZYX(anglesInRadian, center, translation);

    // 先按照1:1 像素生成
    //vtkSmartPointer<vtkMatrix4x4> mUVfrc = vtkMatrix4x4::New();
    double fx = f/dx, fy = f/dy;
    m->Element[0][0] *=fx ;
    m->Element[0][1] *=fx ;
    m->Element[0][2] *=fx ;
    m->Element[0][3] *=fx ;

    m->Element[1][0] *=fy ;
    m->Element[1][1] *=fy ;
    m->Element[1][2] *=fy ;
    m->Element[1][3] *=fy ;


    return m;
}


static void demoProjectPoints()
{

    std::vector<Eigen::Vector3d> pts = {
        {0, 0, 0}, {0, 20, 0},
        {20, 0, 0}, {20, 20, 0},
        {0, 0, 10}, {0, 20, 10},
        {20, 0, 10}, {20, 20, 10},
    };

    Eigen::Vector3d translation = {0, 0, 100};
    Eigen::Vector3d center = {10, 10, 0};
    Eigen::Vector3d angleInDegree = {-90, 0, 0};

    double f = 1;
    double dx = 1, dy = 1; // 1mm 对应1个像素
    double u0 = 20, v0 = 20;
    vtkSmartPointer<vtkMatrix4x4> m = createHMatrix(center, translation, f, dx, dy, angleInDegree);
    m->Print(std::cout);
    for (auto it = pts.begin();it != pts.end(); it++) {
        Eigen::Vector3d proP = multiplyPoint(m, it->data());

        std::cout << "projection: "
                  << proP[0] << " "
                  << proP[1] << " "
                  << proP[2] << " "
                  << std::endl;
    }
}

template<typename _Tp>
int projectImage(vtkImageData *volume, vtkMatrix4x4 *m, std::string &outputFilename, int w, int h, int threshold=1)
{
    int dims[3];
    volume->GetDimensions(dims);
    double range[2];
    volume->GetScalarRange(range);
    double maxvalue = range[1] - threshold;
    double spacing[3];
    volume->GetSpacing(spacing);

    using PixelType = double;
    constexpr unsigned int Dimension = 2;
    using ImageType = itk::Image<PixelType, Dimension>;

    ImageType::RegionType region;
    ImageType::IndexType  start;
    start[0] = 0;
    start[1] = 0;

    ImageType::SizeType size;
    size[0] = w;
    size[1] = h;

    region.SetSize(size);
    region.SetIndex(start);

    ImageType::Pointer image = ImageType::New();
    image->SetRegions(region);

    image->Allocate();
    image->FillBuffer(1);

    _Tp* pData = static_cast<_Tp*>(volume->GetScalarPointer());
    for (int z = 0; z < dims[2]; z++)
    {
        for (int y = 0; y < dims[1]; y++)
        {
            for (int x = 0; x < dims[0]; x++)
            {
                _Tp pixel = pData[x + y*dims[0] + z*dims[0]*dims[1]];
                if (pixel < threshold) {
                    continue;
                }
                //unsigned char newpixel = 255*(pixel - threshold)/maxvalue;
                double newpixel = pixel;
                double p[3] = {double(x*spacing[0]), double(y*spacing[1]), double(z*spacing[2])};
                double out[2];
                projectionPoint(m, p, w/2, h/2, out);

                // 计算4个点
#if 1
                int x1 = floor(out[0]), y1 = floor(out[1]);
                int x2 = x1+1, y2 = y1+1;
                double xs = (out[0]-x1);
                double ys = (out[1]-y1);
                double s1 = ys * xs, s2 = ys*(1-xs), s3=(1-ys)*xs, s4=(1-ys)*(1-xs);

                if (x1 >= 0 && x1<w && y1 >= 0 && y1 < h) {
                    ImageType::IndexType pixelIndex = { { x1, y1 } };
                    ImageType::PixelType pv2 = image->GetPixel(pixelIndex);
                    image->SetPixel(pixelIndex, newpixel*s1+pv2);
                }
                if (x2 >= 0 && x2<w && y1 >= 0 && y1 < h) {
                    ImageType::IndexType pixelIndex = { { x2, y1 } };
                    ImageType::PixelType pv2 = image->GetPixel(pixelIndex);
                    image->SetPixel(pixelIndex, newpixel*s2+pv2);
                }
                if (x1 >= 0 && x1<w && y2 >= 0 && y2 < h) {
                    ImageType::IndexType pixelIndex = { { x1, y2 } };
                    ImageType::PixelType pv2 = image->GetPixel(pixelIndex);
                    image->SetPixel(pixelIndex, newpixel*s3+pv2);
                }
                if (x2 >= 0 && x2<w && y2 >= 0 && y2 < h) {
                    ImageType::IndexType pixelIndex = { { x2, y2 } };
                    ImageType::PixelType pv2 = image->GetPixel(pixelIndex);
                    image->SetPixel(pixelIndex, newpixel*s4+pv2);
                }
#else


                int ox = out[0], oy = out[1];
                if (ox >= 0 && ox<w && oy >= 0 && oy < h) {
                    ImageType::IndexType pixelIndex = { { ox, oy } };
                    ImageType::PixelType pv2 = image->GetPixel(pixelIndex);
                    //std::cout << "xxx:" << pv2 << endl;
                    image->SetPixel(pixelIndex, newpixel+pv2);
//                    if (pv2 < newpixel) {
//image->SetPixel(pixelIndex, newpixel+pv2);
//                    }
                }
#endif
            }
            //std::cout << std::endl;
        }
        //std::cout << std::endl;
    }

    using OutputImageType = itk::Image<unsigned char, Dimension>;
    using FilterType = itk::RescaleIntensityImageFilter<ImageType, OutputImageType>;
    FilterType::Pointer filter = FilterType::New();

    filter->SetOutputMinimum(0);
    filter->SetOutputMaximum(255);
    filter->SetInput(image);


    using WriterType = itk::ImageFileWriter<OutputImageType>;
    WriterType::Pointer writer = WriterType::New();
    writer->SetFileName(outputFilename);
    writer->SetInput(filter->GetOutput());

    try
    {
        writer->Update();
        std::cout << "Write to " << outputFilename << std::endl;
    }
    catch (itk::ExceptionObject & error)
    {
        std::cerr << "Error: " << error << std::endl;
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

int main(int argc, char *argv[])
{

    //demoProjectPoints();
    //return 0;

    std::string inputFilename;
    double threshold = 5;
    int option = 0;
    if(argc < 2)
    {
      std::cerr << "Required arguments: image.mha" << std::endl;
      return EXIT_FAILURE;
    }
    inputFilename = argv[1];
    if (argc > 2) {
        option = atoi(argv[2]);
    }

    if (argc > 3) {
        threshold = atof(argv[3]);
    }

    vtkSmartPointer<vtkMetaImageReader> readerMHa = vtkSmartPointer<vtkMetaImageReader>::New();
    vtkSmartPointer<vtkDICOMImageReader> readerDcm = vtkSmartPointer<vtkDICOMImageReader>::New();
    vtkImageData *volume = nullptr;
    if (option == 0) {
        readerMHa->SetFileName(inputFilename.c_str());
        readerMHa->Update();

        volume = readerMHa->GetOutput();
    } else if (option == 1) {
        readerDcm->SetDirectoryName(inputFilename.c_str());
        readerDcm->Update();
        volume = readerDcm->GetOutput();
    } else {
        cout << "Not supported type." << std::endl;
        return EXIT_FAILURE;
    }

    double origin[3];
    volume->GetOrigin(origin);
    cout << "origin: " << origin[0] << ", " << origin[1] << ", " << origin[2] << endl;
    double spacing[3];
    volume->GetSpacing(spacing);
    cout << "spacing: " << spacing[0] << ", " << spacing[1] << ", " << spacing[2] << endl;
    int dims[3];
    volume->GetDimensions(dims);
    cout << "dimensions: " << dims[0] << ", " << dims[1] << ", " << dims[2] << endl;
    int extent[6];
    volume->GetExtent(extent);
    cout << "extent: " << extent[0] << ", " << extent[1] << ", " << extent[2] << ", " <<
            extent[3] << ", " << extent[4] << ", " << extent[5] << endl;

    // 获取原图像数据的灰度值范围
    // 如：CT值的单位是Hounsfield，简称为Hu，范围是-1024-3071。用于衡量人体组织对X射线的吸收率，设定水的吸收率为0Hu。
    double range[2];
    volume->GetScalarRange(range);
    cout << "range: " << range[0] << ", " << range[1] << endl;
    cout << "GetScalarTypeAsString (): " << volume->GetScalarTypeAsString () << endl;          

    //Eigen::Vector3d focus = {dims[0]/2, dims[1]/2, 100};
    Eigen::Vector3d center = {dims[0]*spacing[0]/2, dims[1]*spacing[1]/2, dims[2]*spacing[2]/2};
    Eigen::Vector3d translation = {0, 0, 1000};
    double f = 1600;
    double dx = 1, dy = 1; // 1mm 对应1个像素

    // 投影图像
    int w = 400, h = 400;
    double u0 = w/2, v0 = h/2;

    for (int angleY=0; angleY < 180; angleY +=10) {
        for (int angleX=0; angleX < 180; angleX +=10) {
            //int angleX = 30;
            Eigen::Vector3d angleInDegree = {angleX, angleY, 0};

            vtkSmartPointer<vtkMatrix4x4> m = createHMatrix(center, translation, f, dx, dy, angleInDegree);
            m->Print(std::cout);
            char buf[200];
            snprintf(buf, 200, "test/proj_%03d_%03d.png", angleY, angleX);
            std::string outputFilename = buf;

            switch (volume->GetScalarType()) {
            case VTK_SHORT:
                projectImage<short>(volume, m, outputFilename, w, h, threshold);
                break;
            default:
                projectImage<unsigned char>(volume, m, outputFilename, w, h);
                break;
            }

            //return 3;
        }
    }

    return 0;
}
